/**
  ******************************************************************************
  * @file           : mcu_application.c
  * Description     : This file provides code for the configuration
  *                   of the TiMcuApplication instances.
  ******************************************************************************
  * @attention
  *	Created by Chen Lu in 20210612
  *
  ******************************************************************************
  */

#include "stdlib.h"
#include "string.h"
#include "math.h"
#include "main.h"
#include "sio_service.h"
#include "motion_service.h"
#include "mcu_application.h"
#include "queue.h"

/**
  * @brief  Open a MCU application.
  *
  * @note   This function opens a MCU application.
  *
  * @retval A MCU application instance pointer.
  */
TiMcuApplication *mcu_app_open(void)
{
	TiMcuApplication *mcu_app = (TiMcuApplication*) malloc(sizeof(TiMcuApplication));
	memset(mcu_app, 0x00, sizeof(TiMcuApplication));
	mcu_app->mcs = mcs_open();
	mcu_app->sio = sio_open();
	mcu_app->state = OPEN;
	QueueInit(&mcu_app->task_seq);
	return mcu_app;
}

/**
  * @brief  Run a MCU application.
  *
  * @note   This function runs a MCU application by running a FSM in the application.
  *
  * @param	mcu_app		A MCU application instance pointer.
  * @retval MCUAPPLICATION status.
  */
uint8_t mcu_app_run(TiMcuApplication* mcu_app)
{
	TiTask task_now;
	uint8_t is_closed = 0;
	float angle_old[3];
//	for (int i=0;i<3;i++)
//		angle_old[i] = (float)(axis_read(mcu_app->mcs->axis[i]) * STEP_ANGLE);

	while (1)
	{
		switch (mcu_app->state)
		{
		case OPEN:
			mcs_returnzero(mcu_app->mcs);
			for (int i=0;i<3;i++)
				angle_old[i] = (float)(axis_read(mcu_app->mcs->axis[i]) * STEP_ANGLE);
			mcu_app->state = IDLE;
			break;
		case IDLE:
			if (QueueSize(&mcu_app->task_seq) != 0)
			{
				if (QueueFront(&mcu_app->task_seq).type == MOV_ROTATE)
					mcu_app->state = RUNNING;
				else if (QueueFront(&mcu_app->task_seq).type == OBJ_CLOSE)
					mcu_app->state = CLOSE;
			}
			break;
		case RUNNING:
			if (QueueSize(&mcu_app->task_seq) != 0)
			{
				task_now = QueuePop(&mcu_app->task_seq);
				excute_task(mcu_app, task_now, angle_old);
			}

			break;
		case STOP:
			if (QueueSize(&mcu_app->task_seq) != 0)
			{
				task_now = QueuePop(&mcu_app->task_seq);
				if (task_now.type == MOV_START)
					excute_task(mcu_app, task_now, angle_old);
				else if (task_now.type == OBJ_CLOSE)
					mcu_app->state = CLOSE;
			}
			break;
		case CLOSE:
			if (is_closed == 0)
			{
				QueueSetEmpty(&mcu_app->task_seq);
				if (mcs_close(mcu_app->mcs) != MOTIONSERVICE_OK)
				{
					printf("%s\r\n","Motion service close error!");
					return MCUAPPLICATION_ERROR;
				}
				is_closed = 1;
			}
			else
			{
				if (QueueSize(&mcu_app->task_seq) != 0)
					task_now = QueuePop(&mcu_app->task_seq);
				if (task_now.type == OBJ_OPEN)
				{
					mcu_app->state = OPEN;
					HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,MOTOR_EN,RESET);
					is_closed = 0;
				}
			}
			break;
		default:
			break;
		}
	}
	mcu_app_close(mcu_app);
	return MCUAPPLICATION_OK;
}

/**
  * @brief  Execute task.
  *
  * @note   This function execute specific task in the FSM of the MCU application.
  *
  * @param	mcu_app		A MCU application instance pointer.
  * @param	task		Task need to be executed.
  * @param	angle		Angles of three axis before executing the task.
  * @retval MCUAPPLICATION status.
  */
uint8_t excute_task(TiMcuApplication* mcu_app, TiTask task, float* angle)
{
	uint8_t end_flag[3] = {0,0,0};
	switch (task.type)
	{
	case MOV_ROTATE:
		for (int i=0;i<3;i++)
		{
			uint8_t direction = 'P';
			if (task.value[i+3] < 0)
			{
				direction = 'N';
				task.value[i+3] = fabs(task.value[i+3]);
			}
			//For the problem it may run into due to the loss between uint16_t and float
			if (task.value[i+3] != 0 && (uint16_t) (task.value[i+3]/STEP_ANGLE) == 0)
			{
				if (axis_write(mcu_app->mcs->axis[i], direction, (uint16_t) 1) != AXIS_OK)
				{
					printf("%s\r\n","Axis return zero write error!");
					return MCUAPPLICATION_ERROR;
				}
			}
			else if (axis_write(mcu_app->mcs->axis[i], direction, (uint16_t) (task.value[i+3]/STEP_ANGLE)) != AXIS_OK)
			{
				printf("%s\r\n","Axis return zero write error!");
				return MCUAPPLICATION_ERROR;
			}
		}
		for (int i=0;i<3;i++)
		{
			if (axis_start(mcu_app->mcs->axis[i]) != AXIS_OK)
			{
				printf("%s\r\n","Axis return zero start error!");
				return MCUAPPLICATION_ERROR;
			}
		}
		//Alpha version
		while (end_flag[0] == 0 || end_flag[1] == 0 || end_flag[2] == 0)
		{
			if (fabs((float)(axis_read(mcu_app->mcs->axis[0]) * STEP_ANGLE) - *(angle+0)) >= task.value[0] && end_flag[0] ==0)
			{
				axis_stop(mcu_app->mcs->axis[0]);
				*(angle+0) = (float)(axis_read(mcu_app->mcs->axis[0]) * STEP_ANGLE);
				end_flag[0] = 1;
			}
			if (fabs((float)(axis_read(mcu_app->mcs->axis[1]) * STEP_ANGLE) - *(angle+1)) >= task.value[1] && end_flag[1] ==0)
			{
				axis_stop(mcu_app->mcs->axis[1]);
				*(angle+1) = (float)(axis_read(mcu_app->mcs->axis[1]) * STEP_ANGLE);
				end_flag[1] = 1;
			}
			if (fabs((float)(axis_read(mcu_app->mcs->axis[2]) * STEP_ANGLE) - *(angle+2)) >= task.value[2] && end_flag[2] ==0)
			{
				axis_stop(mcu_app->mcs->axis[2]);
				*(angle+2) = (float)(axis_read(mcu_app->mcs->axis[2]) * STEP_ANGLE);
				end_flag[2] = 1;
			}
		}
//		for (int i=0;i<3;i++)
//		{
//			while (fabs((float)(axis_read(mcu_app->mcs->axis[i]) * STEP_ANGLE) - *(angle+i)) < task.value[i]);
//			axis_stop(mcu_app->mcs->axis[i]);
//			*(angle+i) = (float)(axis_read(mcu_app->mcs->axis[i]) * STEP_ANGLE);
//		}

		mcu_app->state = RUNNING;
		break;
	case MOV_START:
		mcu_app->state = RUNNING;
		break;
	case MOV_ZERO:
		for (int i=1;i<3;i++)
		{
			if (axis_returnzero(mcu_app->mcs->axis[i])!=AXIS_OK)
			{
				printf("%s\r\n","MCU application axis return zero error!");
				return MCUAPPLICATION_ERROR;
			}
		}
		mcu_app->mcs->axis[0]->encoder = 0;
		//For the cumulative error and the axis with no zero adjuster.
		*angle = 0.0;
		*(angle + 1) = 0.0;
		*(angle + 2) = 0.0;
		mcu_app->state = IDLE;
		break;
	case MOV_STOP:
		for (int i=0;i<3;i++)
		{
			if (axis_stop(mcu_app->mcs->axis[i]) != AXIS_OK)
			{
				printf("%s\r\n","Axis stop error!");
				return MCUAPPLICATION_ERROR;
			}
		}
		//Flush all tasks
		QueueSetEmpty(&mcu_app->task_seq);
		mcu_app->state = STOP;
		break;
	default:
		break;
	}
	return MCUAPPLICATION_OK;
}

/**
  * @brief  Close a MCU application.
  *
  * @note   This function closes a MCU application, in which all instance created in MCU application will be closed.
  * 		So it should be careful when using it.
  *
  * @retval MCUAPPLICATION status.
  */
uint8_t mcu_app_close(TiMcuApplication* mcu_app)
{
	if (mcs_close(mcu_app->mcs) != MOTIONSERVICE_OK)
	{
		printf("%s\r\n","Motion service close error!");
		return MCUAPPLICATION_ERROR;
	}
	//Attention here!
	if (sio_close(mcu_app->sio) != SIOSERVICE_OK)
	{
		printf("%s\r\n","SIO service close error!");
		return MCUAPPLICATION_ERROR;
	}
	mcu_app->state = CLOSE;
	//Attention here!
	QueueDestory(&mcu_app->task_seq);
	memset(mcu_app->request,0x00,127);
	memset(mcu_app->response,0x00,127);
	mcu_app->request_length = 0;
	return MCUAPPLICATION_OK;
}

